#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Utils.h>
#include <ceres/ceres.h>
#include <sophus/so3.hpp>
using namespace std;
using namespace Eigen;
namespace xio=lyxutils::io;
namespace xos=lyxutils::os;

void print_usage(){
    cout<<"usage:estimateT ref_pts.txt align_pts.txt [-o result.txt]"<<endl;
    cout<<"explain:esitimate the best similarity transformation matrix between "<<endl;
    cout<<"        the given two point sets(from align pts to ref pts, namely T_{ref,align})"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h              print this help message"<<endl;
    cout<<"  -o=result.txt   set output file name(default is result.txt)"<<endl;
}

struct PosErrorCost{//变换后点位置与参照点误差成本函数
    PosErrorCost(double x,double y,double z,double x_ref,double y_ref,double z_ref):
    _x(x),_y(y),_z(z),_x_ref(x_ref),_y_ref(y_ref),_z_ref(z_ref){}
    //残差计算
    bool operator()(const double *const phi_t_alpha, double *residuals)const{
        //模型参数phi_t_alpha,有7个，前三个是李代数,后三个是平移量,最后一个是缩放
        Vector3d vec_so3(phi_t_alpha[0], phi_t_alpha[1], phi_t_alpha[2]);
        Matrix3d R=Sophus::SO3d::exp(vec_so3).matrix();
        Vector3d X_align(_x,_y,_z);
        Vector3d t(phi_t_alpha[3],phi_t_alpha[4],phi_t_alpha[5]);
        double alpha=phi_t_alpha[6];
        Vector3d X_trans=alpha*R*X_align+t;
        residuals[0]=_x_ref-X_trans[0];
        residuals[1]=_y_ref-X_trans[1];
        residuals[2]=_z_ref-X_trans[2];
        return true;
    }

    const double _x,_y,_z;
    const double _x_ref,_y_ref,_z_ref;
};

int main(int argc,char **argv) {
    xio::CLParser parser;
    try{
        parser.parse(argc,argv);
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        print_usage();
        return 1;
    }
    if(argc<3||parser.hasOption("h")){
        print_usage();
        return 1;
    }
    vector<string> parameters=parser.getParameters();
    vector<vector<float> > ref_pts,align_pts;
    for(int i=0;i<2;++i)if(!xos::file(xos::abspath(parameters[i]))){
        cout<<"file:"<<parameters[i]<<" not exist!"<<endl;
        return 1;
    }//test file existence
    xio::read_csv(parameters[0],ref_pts,", ");
    xio::read_csv(parameters[1], align_pts,", ");
    string outputFile="result.txt";
    if(parser.hasOption("o")&&parser.getOptionArg("o")!=""){
        outputFile=parser.getOptionArg("o");
        if(xos::file(outputFile)){
            cout<<"output file:"<<outputFile<<" already exist,change to default:";
            outputFile="result.txt";
            cout<<outputFile<<endl;
        }
    }
    //do some real work
    //构建问题
    double phi_t_alpha[7]={0};
    cout<<"初始参数:";
    for(int i=0;i<7;++i)cout<<phi_t_alpha[i]<<",";
    cout<<endl;
    ceres::Problem problem;
    int N=ref_pts.size()<align_pts.size()?ref_pts.size():align_pts.size();
    for(int i=0;i<N;++i){
        problem.AddResidualBlock(//自动求导,模板参数依次为成本函数类型,残差个数,参数个数
                new ceres::NumericDiffCostFunction<PosErrorCost,ceres::RIDDERS,3,7>(
                        new PosErrorCost(align_pts[i][0],align_pts[i][1],align_pts[i][2],
                                ref_pts[i][0],ref_pts[i][1],ref_pts[i][2])
                                ),
                nullptr,
                phi_t_alpha
                );
    }
    //配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout=true;//输出到cout
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);
    //输出结果
    cout<<summary.BriefReport()<<endl;
    Matrix3d R=Sophus::SO3d::exp(Vector3d(phi_t_alpha[0],phi_t_alpha[1],phi_t_alpha[2])).matrix();
    Vector3d t(phi_t_alpha[3],phi_t_alpha[4],phi_t_alpha[5]);
    double alpha=phi_t_alpha[6];
    cout<<"result:"<<endl;
    double RMS=sqrt(summary.final_cost/(N-1));
    cout<<"RMS="<<RMS<<endl;
    cout<<"alpha="<<alpha<<endl;
    cout<<"t=("<<t(0)<<","<<t(1)<<","<<t(2)<<")"<<endl;
    cout<<"so3=("<<phi_t_alpha[0]<<","<<phi_t_alpha[1]<<","<<phi_t_alpha[2]<<")"<<endl;
    cout<<"R="<<endl;
    cout<<R<<endl;
    R=alpha*R;
    cout<<"T="<<endl;
    Matrix4d T;
    T.topLeftCorner(3,3)=R;
    T.row(3)=Matrix<double,1,4>(0,0,0,1);
    T.topRightCorner(3,1)=t;
    cout<<T<<endl;
    ofstream ofs(outputFile);
    ofs<<setprecision(8);
    for(int i=0;i<T.rows();++i){
        ofs<<T(i,0);
        for(int j=1;j<T.cols();++j){
            ofs<<","<<T(i,j);
        }
        ofs<<endl;
    }
    ofs.flush();
    if(ofs.is_open())ofs.close();
    cout<<"result saved to file:"<<outputFile<<endl;
    return 0;
}